﻿using System;
using System.Collections.Generic;
using Algoritmia.DataStructures.Digraphs;
using Algoritmia.DataStructures.Maps;
using Algoritmia.DataStructures.PriorityMaps;
using Algoritmia.Utils;
using Sets = Algoritmia.DataStructures.Sets;
using Algoritmia.Utils.Backtracers;

namespace Algoritmia.Problems.ShortestPaths.Metric
{
    public class MetricDigraphShortestPaths<TNode>
    {
        private readonly WeightingFunction<TNode, double> _h;
        private readonly Func<IEnumerable<TNode>, IMap<TNode, TNode>> _createMap;
        private readonly Func<IEnumerable<TNode>, Sets.ISet<TNode>> _createSet;
        private readonly Func<IEnumerable<KeyValuePair<TNode,double>>,
            IEnumerable<TNode>, IPriorityMap<TNode, double>> _createPriorityMap;
        private readonly Func<IEnumerable<TNode>, IMap<TNode, double>> _createDistancesMap;
        private readonly EqualityComparison<TNode> _equalityComparison;

        public MetricDigraphShortestPaths(WeightingFunction<TNode,double> h,
            EqualityComparison<TNode> equalityComparison = null,
            Func<IEnumerable<TNode>,IMap<TNode,TNode>> createMap = null,
            Func<IEnumerable<TNode>,IMap<TNode,double >> createDistancesMap = null,
            Func<IEnumerable<TNode>,Sets.ISet<TNode>> createSet = null,
            Func<IEnumerable<KeyValuePair<TNode,double>>, IEnumerable<TNode>,IPriorityMap<TNode,double>> createPriorityMap = null)
        {
            _h = h;
            _createMap = createMap ?? (it => AlgoritmiaFactory.Maps<TNode, TNode>.Default);
            _createDistancesMap = createDistancesMap ?? (it => AlgoritmiaFactory.Maps<TNode, double>.Default);
            _createSet = createSet ?? (it => AlgoritmiaFactory.Sets<TNode>.Default );
            _createPriorityMap = createPriorityMap ?? ((d,it) => new MinHeapMap<TNode, double>(d));
            _equalityComparison = AlgoritmiaFactory.GetDefaultEqualityComparison(equalityComparison);
        }

        public IEnumerable<TNode> ShortestPath(IDigraph<TNode> g, WeightingFunction<TNode,double> d, TNode source, TNode target,
            Func<IEnumerable<KeyValuePair<TNode,TNode>>,IBacktracer<TNode,TNode>> createBacktracer = null)
        {
            createBacktracer = createBacktracer ?? (it => new Backtracer<TNode>(it));

            var added = _createSet(g.V);
            var distances = _createDistancesMap(g.V);

            foreach (var v in g.V)
            {
                distances[v] = double.PositiveInfinity;
            }

            distances[source] = 0.0;
            var fringe = _createPriorityMap(distances.Items(),g.V);
            fringe[source] = _h(source, target);
            var back = _createMap(g.V);
            back[source] = source;

            while ( fringe.Count > 0)
            {
                var bestVertex = fringe.ExtractOptimalKey();
                added.Add(bestVertex);
                if (_equalityComparison(bestVertex,target))
                {
                    break;
                }
                foreach(var w in g.Successors(bestVertex))
                {
                    if ( !added.Contains(w) && distances[bestVertex] + d(bestVertex,w) < distances[w])
                    {
                        distances[w] = distances[bestVertex] + d(bestVertex, w);
                        fringe[w] = distances[w] + _h(w, target);
                        back[w] = bestVertex;
                    }
                }
            }
            return createBacktracer(back).Backtrace(target);
        }

    }
}
